import open3d as o3d
import numpy as np


# 创建一个点云示例
def create_point_cloud_example():
    # 生成一些随机点
    points = np.random.rand(1000, 3)  # 1000个随机点
    point_cloud = o3d.geometry.PointCloud()
    point_cloud.points = o3d.utility.Vector3dVector(points)

    # 为点云添加随机颜色
    colors = np.random.rand(1000, 3)
    point_cloud.colors = o3d.utility.Vector3dVector(colors)

    return point_cloud


def main():
    # 创建示例点云
    pcd = create_point_cloud_example()
    print("原始点云包含 {} 个点".format(len(pcd.points)))

    # 可视化原始点云
    o3d.visualization.draw_geometries([pcd],
                                      window_name="原始点云",
                                      width=800,
                                      height=600)

    # 对点云进行下采样
    downpcd = pcd.voxel_down_sample(voxel_size=0.05)
    print("下采样后点云包含 {} 个点".format(len(downpcd.points)))

    # 估计法线
    downpcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

    # 可视化下采样后的点云和法线
    o3d.visualization.draw_geometries([downpcd],
                                      window_name="下采样点云与法线",
                                      point_show_normal=True,
                                      width=800,
                                      height=600)

    # 保存点云到文件
    o3d.io.write_point_cloud("sample_point_cloud.ply", downpcd)
    print("点云已保存到 sample_point_cloud.ply")

    # 从文件读取点云
    loaded_pcd = o3d.io.read_point_cloud("sample_point_cloud.ply")
    print("从文件读取的点云包含 {} 个点".format(len(loaded_pcd.points)))

    # 创建坐标系网格
    mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(
        size=0.6, origin=[0, 0, 0])

    # 同时显示点云和坐标系
    o3d.visualization.draw_geometries([loaded_pcd, mesh_frame],
                                      window_name="点云与坐标系",
                                      width=800,
                                      height=600)


if __name__ == "__main__":
    main()